#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :L1Controller.py
@Description  :
@Time         :2024/05/23 21:31:07
@Author       :Lin Yuheng
@Version      :1.0
'''

from tools.mathlib import *
from tools.vector import *
from tools.filter import *

class L1Controller:
    def __init__(self):
        self._nav_bearing = 0
        self._lateral_accel = 0
        self._k_L1 = 2
        self._L1_period = 25
        self._L1_ratio = 5.0
        self._L1_damping = 0.75
        self._L1_distance = 20.0
        self._roll_lim_rad = math.pi / 3

    def set_l1_period(self, period):
        self._L1_period = period
        self._L1_ratio = 1.0 / math.pi * self._L1_damping * self._L1_period

    def set_l1_damping(self, damping):
        self._L1_damping = damping
        self._L1_ratio = 1.0 / math.pi * self._L1_damping * self._L1_period
        self._k_L1 = 4.0 * self._L1_damping * self._L1_damping

    def set_l1_roll_limit(self, roll_lim_rad):
        self._roll_lim_rad = roll_lim_rad

    def nav_roll(self):
        ret = math.atan(self._lateral_accel / 9.8)
        ret = constrain(ret, -self._roll_lim_rad, self._roll_lim_rad)
        return ret

    def nav_lateral_accleration_demand(self):
        return self._lateral_accel

    def nav_bearing(self):
        return self._nav_bearing

    def l1_controller(
        self, current_pos: Point, sp_pos: Point, ground_speed_2d: Point, airspeed: float
    ):

        # enforce a minimum ground speed of 0.1 m/s to avoid singularities
        ground_speed = max(ground_speed_2d.len(), 0.1)

        self._L1_distance = self._L1_ratio * ground_speed

        # 计算目标点到飞机的向量 ned
        vector_A_to_airplane = Point(current_pos.x - sp_pos.x, current_pos.y - sp_pos.y)

        # 计算飞机到waypoint A的eta
        vector_A_to_airplane_unit = vector_A_to_airplane.normlized()

        # velocity across / orthogonal to line
        xtrack_vel = ground_speed_2d.cross(vector_A_to_airplane_unit.reverse())

        # velocity along line
        ltrack_vel = ground_speed_2d.dot(vector_A_to_airplane_unit.reverse())

        eta = math.atan2(xtrack_vel, ltrack_vel)

        # 计算当前位置到L1点的方位
        self._nav_bearing = math.atan2(
            -vector_A_to_airplane_unit.y, -vector_A_to_airplane_unit.x
        )

        # limit angle to +- 90 degree
        eta = constrain(eta, -math.pi / 2, math.pi / 2)

        # 计算L1控制器的横向加速度
        self._lateral_accel = (
            self._k_L1 * ground_speed * ground_speed / self._L1_distance * math.sin(eta)
        )
